/**
 * @file loam_registration.cpp
 * @author ysh
 * @brief LOAM 匹配模块
 * @version 0.1
 * @date 2023-05-10
 * 
 * @copyright Copyright (c) 2023
 * 
 */
#include "../../../include/models/registration/loam/loam_registration.hpp"
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <cmath>
#include <algorithm>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/SVD>
//#include "glog/logging.h"
using namespace std;

const double scanPeriod = 0.1;
double MINIMUM_RANGE = 0.1;
int N_SCANS = 64;
std::string LIDAR_TYPE="HDL64";
double DISTANCE_SQ_THRESHOLD = 25;
double NEARBY_SCAN = 2.5;

//bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
template <typename PointT>
static void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{
    if (&cloud_in != &cloud_out)
    {
        cloud_out.header = cloud_in.header;
        cloud_out.points.resize(cloud_in.points.size());
    }

    size_t j = 0;

    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {
        if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
            continue;
        cloud_out.points[j] = cloud_in.points[i];
        j++;
    }
    if (j != cloud_in.points.size())
    {
        cloud_out.points.resize(j);
    }

    cloud_out.height = 1;
    cloud_out.width = static_cast<uint32_t>(j);
    cloud_out.is_dense = true;
}


LOAMRegistration::LOAMRegistration() : 
kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>()),
kdtreeSurfLast(new pcl::KdTreeFLANN<pcl::PointXYZI>()) {}

bool LOAMRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    input_target_ = input_target;
    CloudData::CLOUD cornerPointsSharp,cornerPointsLessSharp,
                        surfPointsFlat,surfPointsLessFlat;
    ExtractFullFeaturePoints(input_target_,
                        cornerPointsSharp,
                        cornerPointsLessSharp,
                        surfPointsFlat,
                        surfPointsLessFlat);  
    last_cornerPointsLessSharp =  cornerPointsLessSharp.makeShared();     
    last_surfPointsLessFlat = surfPointsLessFlat.makeShared();

    kdtreeCornerLast->setInputCloud(last_cornerPointsLessSharp);
    kdtreeSurfLast->setInputCloud(last_surfPointsLessFlat);
    return true;
}

bool LOAMRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                                const Eigen::Matrix4f& predict_pose, 
                                CloudData::CLOUD_PTR& result_cloud_ptr,
                                Eigen::Matrix4f& result_pose) {
    //|提取点云特征
    input_source_ = input_source;
    CloudData::CLOUD current_cornerPointsSharp,
                     current_surfPointsFlat;
    ExtractFeaturePoints(input_source_,
                         current_cornerPointsSharp,
                         current_surfPointsFlat);
    //.将角点和平面点变换到预测值上
    CloudData::CLOUD_PTR transformed_input_cornerPoints(new CloudData::CLOUD());
    CloudData::CLOUD_PTR transformed_input_surfPoints(new CloudData::CLOUD());
    pcl::transformPointCloud(current_cornerPointsSharp, *transformed_input_cornerPoints, predict_pose);
    pcl::transformPointCloud(current_surfPointsFlat, *transformed_input_surfPoints, predict_pose);

    transformation_.setIdentity();
    int curr_iter = 0;
    while (curr_iter < 10) {
       
        CloudData::CLOUD_PTR current_input_cornerPoints(new CloudData::CLOUD());
        CloudData::CLOUD_PTR current_input_surfPoints(new CloudData::CLOUD());
        pcl::transformPointCloud(*transformed_input_cornerPoints, *current_input_cornerPoints, transformation_);
        pcl::transformPointCloud(*transformed_input_surfPoints, *current_input_surfPoints, transformation_);

        //cout<<current_cornerPointsSharp.size()<<endl;
        static double para_q[4] = {0,0,0,1};
        static double para_t[3] = {0,0,0};
        ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
        ceres::LocalParameterization *q_parameterization =
            new ceres::EigenQuaternionParameterization();
        // ceres::LocalParameterization *q_parameterization =
        //     new PoseSO3Parameterization();
        ceres::Problem::Options problem_options;
        ceres::Problem problem(problem_options);
        problem.AddParameterBlock(para_q, 4, q_parameterization);
        problem.AddParameterBlock(para_t, 3);
        FindCorresondenceForCornerFeatures(*current_input_cornerPoints);
        for(int i=0;i<curr_cornerpoints.size();i++)
        {
           
           //ceres::CostFunction *cost_function = LidarEdgeFactorAuto::Create(curr_cornerpoints[i], last_cornerpoints_a[i], last_cornerpoints_b[i], 1);
           ceres::CostFunction *cost_function = new LidarEdgeFactorAnaly(curr_cornerpoints[i], last_cornerpoints_a[i], last_cornerpoints_b[i], 1);
           problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
        }
        FindCorresondenceForPlaneFeatures(*current_input_cornerPoints); 
        for(int i=0;i<curr_surfpoints.size();i++)
        {
            //ceres::CostFunction *cost_function = LidarPlaneFactorAuto::Create(curr_surfpoints[i], last_surfpoints_a[i], last_surfpoints_b[i], last_surfpoints_c[i],1);
            ceres::CostFunction *cost_function = new LidarPlaneFactorAnaly(curr_surfpoints[i], last_surfpoints_a[i], last_surfpoints_b[i], last_surfpoints_c[i],1);
            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
        }
        if ((curr_cornerpoints.size() + curr_surfpoints.size()) < 10)
        {
            printf("less correspondence! *************************************************\n");
            continue;
        }
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.max_num_iterations = 10;
        options.minimizer_progress_to_stdout = true;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        cout<<summary.FullReport()<<endl;

    
        //cout<<summary.FullReport()<<endl;

        Eigen::Quaternionf q_last_curr(static_cast<float>(para_q[3]),
        static_cast<float>(para_q[0]),
        static_cast<float>(para_q[1]),
        static_cast<float>(para_q[2]));
        Eigen::Vector3f t_last_curr(    static_cast<float>(para_t[0]),
        static_cast<float>(para_t[1]),
        static_cast<float>(para_t[2]));
        Eigen::Matrix4f deltatransformation_;
        deltatransformation_.setIdentity();
        deltatransformation_.block<3,3>(0,0) = q_last_curr.toRotationMatrix();
        deltatransformation_.block<3,1>(0,3) = t_last_curr;
        if(!IsSignificant(deltatransformation_,0.01)) break;
        transformation_ = deltatransformation_*transformation_;

         curr_iter++;
    }
    std::cout<<curr_iter<<std::endl;
    result_pose = transformation_ * predict_pose;
    Eigen::Quaternionf  qr(result_pose.block<3,3>(0,0));
    qr.normalize();
    Eigen::Vector3f  t  = result_pose.block<3,1>(0,3);
    result_pose.setIdentity();
    result_pose.block<3,3>(0,0) = qr.toRotationMatrix();
    result_pose.block<3,1>(0,3) = t;

    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);
    
    return true;
}

/**
 * @brief 提取点云的角点和平面点
 * 
 * @param[in] input_source 输入点云
 * @param[in] cornerPointsSharp 角点点云
 * @param[in] cornerPointsLessSharp 次角点点云
 * @param[in] surfPointsFlat 平面点点云
 * @param[in] surfPointsLessFlat 次平面点点云
 * @return true 
 * @return false 
 */
//!记录一个每想明白的问题
//!角点和平面点云用指帧类型创建会报错
bool LOAMRegistration::ExtractFullFeaturePoints(const CloudData::CLOUD_PTR& input_source,
                                  CloudData::CLOUD& cornerPointsSharp,
                                  CloudData::CLOUD& cornerPointsLessSharp,
                                  CloudData::CLOUD& surfPointsFlat,
                                  CloudData::CLOUD& surfPointsLessFlat){
    CloudData::CLOUD laserCloudIn = *input_source;
/*********************************************提取特征点预处理****************************************/    
    //|去除无效点
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
    int cloudSize = laserCloudIn.points.size();
    //|计算起始点和终止点角度
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;
     //std::cout<<startOri<<" "<<endOri<<std::endl;
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }
    //|计算每个点的角度和loam自定义的点的强度
    bool halfPassed = false;
    int count = cloudSize;
    CloudData::POINT point;
    std::vector<CloudData::CLOUD> laserCloudScans(N_SCANS);

    for (int i = 0; i < cloudSize; i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
        int scanID = 0;

        if (LIDAR_TYPE == "VLP16" && N_SCANS == 16)
        {
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (LIDAR_TYPE == "HDL32" && N_SCANS == 32)
        {
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        // HDL64 (e.g., KITTI)
        else if (LIDAR_TYPE == "HDL64" && N_SCANS == 64)
        {   
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
                count--;
                continue;
            }
        }
        // Ouster OS1-64 (e.g., MulRan)
        else if (LIDAR_TYPE == "OS1-64" && N_SCANS == 64)
        {   
            scanID = int((angle + 22.5) / 2 + 0.5); // ouster os1-64 vfov is [-22.5, 22.5] see https://ouster.com/products/os1-lidar-sensor/
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else
        {
            printf("wrong scan number\n");
            //ROS_BREAK();
        }
        ////printf("angle %f scanID %d \n", angle, scanID);

        float ori = -atan2(point.y, point.x);
        if (!halfPassed)
        { 
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }

        float relTime = (ori - startOri) / (endOri - startOri);
        point.intensity = scanID + scanPeriod * relTime;
        laserCloudScans[scanID].push_back(point); 
    }
    
    cloudSize = count;
    printf("points size %d \n", cloudSize);
    
    
    //|1.获得经过上步骤处理后点点云
    //|2.获得每条线的第5个和倒数第5个点的ID
    CloudData::CLOUD_PTR laserCloud(new CloudData::CLOUD);
    std::vector<int> scanStartInd(N_SCANS, 0);
    std::vector<int> scanEndInd(N_SCANS, 0);
    for (int i = 0; i < N_SCANS; i++)
    { 
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6;
    }
/*********************************************提取特征点预处理****************************************/    
/*********************************************计算每个点的曲率****************************************/    
    
    float cloudCurvature[400000];
    int cloudSortInd[400000];
    int cloudNeighborPicked[400000];
    int cloudLabel[400000];

    for (int i = 5; i < cloudSize - 5; i++)
    { 
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        cloudSortInd[i] = i;
        cloudNeighborPicked[i] = 0;
        cloudLabel[i] = 0;
    }   
/*********************************************计算每个点的曲率****************************************/    
    
/*********************************************开始提取特征点****************************************/    
    for (int i = 0; i < N_SCANS; i++)
        {
            if( scanEndInd[i] - scanStartInd[i] < 6)
                continue;
            CloudData::CLOUD_PTR surfPointsLessFlatScan(new CloudData::CLOUD);
            //|将一条扫描线分为6段
            for (int j = 0; j < 6; j++)
            {
                int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
                int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

                //|将每段点按曲率大小按照从小到大进行排序
                //ysh
                int a = cloudSortInd[sp];
                int b = cloudSortInd[ep];
                std::sort(cloudCurvature+a, cloudCurvature+b+1, 
                          [](float i, float j) {
                           return (i<j);
                           }
                           );

                int largestPickedNum = 0;
                for (int k = ep; k >= sp; k--)
                {
                    int ind = cloudSortInd[k]; 
                    //|如果该点没有被标记且曲率大于角点阈值则继续
                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] > 0.1)
                    {

                        largestPickedNum++;
                        //|提取2个角点
                        if (largestPickedNum <= 2)
                        {                        
                            cloudLabel[ind] = 2;
                            cornerPointsSharp.push_back(laserCloud->points[ind]);
                            cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                        }
                        //|提取20个角点                  
                        else if (largestPickedNum <= 20)
                        {                        
                            cloudLabel[ind] = 1; 
                            cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                        }
                        else
                        {
                            break;
                        }
                        //|标记
                        cloudNeighborPicked[ind] = 1; 

                        //|将角点前后五个点标记,保证角点均匀分布
                        for (int l = 1; l <= 2; l++)
                        {
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -2; l--)
                        {
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
                int smallestPickedNum = 0;
                for (int k = sp; k <= ep; k++)
                {
                    int ind = cloudSortInd[k];

                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] < 0.1)
                    {

                        cloudLabel[ind] = -1; 
                        //|提取四个平面点
                        surfPointsFlat.push_back(laserCloud->points[ind]);

                        smallestPickedNum++;
                        if (smallestPickedNum >= 4)
                        { 
                            break;
                        }

                        cloudNeighborPicked[ind] = 1;
                        for (int l = 1; l <= 2; l++)
                        { 
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -2; l--)
                        {
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
                //|将所有标记为平面点的点存储,用于降采样
                for (int k = sp; k <= ep; k++)
                {
                    //!这里将所有非角点标记为平面点
                    if (cloudLabel[k] <= 0)
                    {
                        surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                    }
                }
            }

            CloudData::CLOUD surfPointsLessFlatScanDS;
            pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
            downSizeFilter.setInputCloud(surfPointsLessFlatScan);
            downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
            downSizeFilter.filter(surfPointsLessFlatScanDS);

            surfPointsLessFlat += surfPointsLessFlatScanDS;
        }
/*********************************************开始提取特征点****************************************/    
     
    return true ;
}
bool LOAMRegistration::ExtractFeaturePoints(const CloudData::CLOUD_PTR& input_source,
                                  CloudData::CLOUD& cornerPointsSharp,
                                  CloudData::CLOUD& surfPointsFlat){
    CloudData::CLOUD laserCloudIn = *input_source;
/*********************************************提取特征点预处理****************************************/    
    //|去除无效点
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
    int cloudSize = laserCloudIn.points.size();
    //|计算起始点和终止点角度
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;
     //std::cout<<startOri<<" "<<endOri<<std::endl;
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }
    //|计算每个点的角度和loam自定义的点的强度
    bool halfPassed = false;
    int count = cloudSize;
    CloudData::POINT point;
    std::vector<CloudData::CLOUD> laserCloudScans(N_SCANS);

    for (int i = 0; i < cloudSize; i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
        int scanID = 0;

        if (LIDAR_TYPE == "VLP16" && N_SCANS == 16)
        {
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (LIDAR_TYPE == "HDL32" && N_SCANS == 32)
        {
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        // HDL64 (e.g., KITTI)
        else if (LIDAR_TYPE == "HDL64" && N_SCANS == 64)
        {   
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
                count--;
                continue;
            }
        }
        // Ouster OS1-64 (e.g., MulRan)
        else if (LIDAR_TYPE == "OS1-64" && N_SCANS == 64)
        {   
            scanID = int((angle + 22.5) / 2 + 0.5); // ouster os1-64 vfov is [-22.5, 22.5] see https://ouster.com/products/os1-lidar-sensor/
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else
        {
            printf("wrong scan number\n");
            //ROS_BREAK();
        }
        ////printf("angle %f scanID %d \n", angle, scanID);

        float ori = -atan2(point.y, point.x);
        if (!halfPassed)
        { 
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }

        float relTime = (ori - startOri) / (endOri - startOri);
        point.intensity = scanID + scanPeriod * relTime;
        laserCloudScans[scanID].push_back(point); 
    }
    
    cloudSize = count;
    printf("points size %d \n", cloudSize);
    
    
    //|1.获得经过上步骤处理后点点云
    //|2.获得每条线的第5个和倒数第5个点的ID
    CloudData::CLOUD_PTR laserCloud(new CloudData::CLOUD);
    std::vector<int> scanStartInd(N_SCANS, 0);
    std::vector<int> scanEndInd(N_SCANS, 0);
    for (int i = 0; i < N_SCANS; i++)
    { 
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6;
    }
/*********************************************提取特征点预处理****************************************/    
/*********************************************计算每个点的曲率****************************************/    
    
    float cloudCurvature[400000];
    int cloudSortInd[400000];
    int cloudNeighborPicked[400000];
    int cloudLabel[400000];

    for (int i = 5; i < cloudSize - 5; i++)
    { 
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        cloudSortInd[i] = i;
        cloudNeighborPicked[i] = 0;
        cloudLabel[i] = 0;
    }   
/*********************************************计算每个点的曲率****************************************/    
    
/*********************************************开始提取特征点****************************************/    
    for (int i = 0; i < N_SCANS; i++)
        {
            if( scanEndInd[i] - scanStartInd[i] < 6)
                continue;
            //|将一条扫描线分为6段
            for (int j = 0; j < 6; j++)
            {
                int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
                int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

                //|将每段点按曲率大小按照从小到大进行排序
                //ysh
                int a = cloudSortInd[sp];
                int b = cloudSortInd[ep];
                std::sort(cloudCurvature+a, cloudCurvature+b+1, 
                          [](float i, float j) {
                           return (i<j);
                           }
                           );
                int largestPickedNum = 0;
                for (int k = ep; k >= sp; k--)
                {
                    int ind = cloudSortInd[k]; 
                    //|如果该点没有被标记且曲率大于角点阈值则继续
                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] > 0.1)
                    {

                        largestPickedNum++;
                        //|提取2个角点
                        if (largestPickedNum <= 2)
                        {                        
                            cloudLabel[ind] = 2;
                            cornerPointsSharp.push_back(laserCloud->points[ind]);
                        }
                        else
                        {
                            break;
                        }
                        //|标记
                        cloudNeighborPicked[ind] = 1; 

                        //|将角点前后五个点标记,保证角点均匀分布
                        for (int l = 1; l <= 5; l++)
                        {
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -5; l--)
                        {
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
                int smallestPickedNum = 0;
                for (int k = sp; k <= ep; k++)
                {
                    int ind = cloudSortInd[k];

                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] < 0.1)
                    {

                        cloudLabel[ind] = -1; 
                        //|提取四个平面点
                        surfPointsFlat.push_back(laserCloud->points[ind]);

                        smallestPickedNum++;
                        if (smallestPickedNum >= 4)
                        { 
                            break;
                        }

                        cloudNeighborPicked[ind] = 1;
                        for (int l = 1; l <= 5; l++)
                        { 
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -5; l--)
                        {
                            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
            }
        }
/*********************************************开始提取特征点****************************************/    
     
    return true ;
}
bool LOAMRegistration::FindCorresondenceForCornerFeatures(CloudData::CLOUD& current_corner_Points)
{
     int corner_correspondence = 0;
     CloudData::CLOUD_PTR current_cornerPointsSharp;
     current_cornerPointsSharp = current_corner_Points.makeShared(); 
     int cornerPointsSharpNum = current_cornerPointsSharp->points.size();
     std::vector<int> pointSearchInd;
     std::vector<float> pointSearchSqDis;
     for (int i = 0; i < cornerPointsSharpNum; ++i)
     {
       // TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
        //kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
        kdtreeCornerLast->nearestKSearch(current_cornerPointsSharp->points[i],1,pointSearchInd, pointSearchSqDis);
        int closestPointInd = -1, minPointInd2 = -1;
        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
        {
            closestPointInd = pointSearchInd[0];
            int closestPointScanID = int(last_cornerPointsLessSharp->points[closestPointInd].intensity);
            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
            for (int j = closestPointInd + 1; j < (int)last_cornerPointsLessSharp->points.size(); ++j)
            {
                // if in the same scan line, continue
                if (int(last_cornerPointsLessSharp->points[j].intensity) <= closestPointScanID)
                    continue;

                // if not in nearby scans, end the loop
                if (int(last_cornerPointsLessSharp->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                    break;

                double pointSqDis = (last_cornerPointsLessSharp->points[j].x - current_cornerPointsSharp->points[i].x) *
                                        (last_cornerPointsLessSharp->points[j].x - current_cornerPointsSharp->points[i].x) +
                                    (last_cornerPointsLessSharp->points[j].y - current_cornerPointsSharp->points[i].y) *
                                        (last_cornerPointsLessSharp->points[j].y - current_cornerPointsSharp->points[i].y) +
                                    (last_cornerPointsLessSharp->points[j].z - current_cornerPointsSharp->points[i].z) *
                                        (last_cornerPointsLessSharp->points[j].z - current_cornerPointsSharp->points[i].z);

                if (pointSqDis < minPointSqDis2)
                {
                    // find nearer point
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
            }
            for (int j = closestPointInd - 1; j >= 0; --j)
            {
                // if in the same scan line, continue
                if (int(last_cornerPointsLessSharp->points[j].intensity) >= closestPointScanID)
                    continue;

                // if not in nearby scans, end the loop
                if (int(last_cornerPointsLessSharp->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                    break;

                double pointSqDis = (last_cornerPointsLessSharp->points[j].x - current_cornerPointsSharp->points[i].x) *
                                        (last_cornerPointsLessSharp->points[j].x - current_cornerPointsSharp->points[i].x) +
                                    (last_cornerPointsLessSharp->points[j].y - current_cornerPointsSharp->points[i].y) *
                                        (last_cornerPointsLessSharp->points[j].y - current_cornerPointsSharp->points[i].y) +
                                    (last_cornerPointsLessSharp->points[j].z - current_cornerPointsSharp->points[i].z) *
                                        (last_cornerPointsLessSharp->points[j].z - current_cornerPointsSharp->points[i].z);

                if (pointSqDis < minPointSqDis2)
                {
                    // find nearer point
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
                
            }
        }
        if(minPointInd2>=0)
        {
            //|当前帧的点
            Eigen::Vector3d curr_point(current_cornerPointsSharp->points[i].x,
                                       current_cornerPointsSharp->points[i].y,
                                       current_cornerPointsSharp->points[i].z);
            curr_cornerpoints.push_back(curr_point);
            Eigen::Vector3d last_point_a(last_cornerPointsLessSharp->points[closestPointInd].x,
                                         last_cornerPointsLessSharp->points[closestPointInd].y,
                                         last_cornerPointsLessSharp->points[closestPointInd].z); 
            last_cornerpoints_a.push_back(last_point_a) ;          
            Eigen::Vector3d last_point_b(last_cornerPointsLessSharp->points[minPointInd2].x,
                                         last_cornerPointsLessSharp->points[minPointInd2].y,
                                         last_cornerPointsLessSharp->points[minPointInd2].z); 
            last_cornerpoints_b.push_back(last_point_b);           
            //ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
           // problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
            corner_correspondence++;
        }
     } 
     //cout<<corner_correspondence<<endl;
     return true;
}

bool LOAMRegistration::FindCorresondenceForPlaneFeatures(CloudData::CLOUD& current_surf_Points){
     int surf_correspondence = 0;
     CloudData::CLOUD_PTR current_surfPointsSharp;
     current_surfPointsSharp = current_surf_Points.makeShared(); 
     int surfPointsSharpNum = current_surfPointsSharp->points.size();
     std::vector<int> pointSearchInd;
     std::vector<float> pointSearchSqDis;
     for (int i = 0; i < surfPointsSharpNum; ++i)
     {
        kdtreeSurfLast->nearestKSearch(current_surfPointsSharp->points[i], 1, pointSearchInd, pointSearchSqDis);
        
        int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
        {
            closestPointInd = pointSearchInd[0];
            int closestPointScanID = int(last_surfPointsLessFlat->points[closestPointInd].intensity);
            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
            for (int j = closestPointInd + 1; j < (int)last_surfPointsLessFlat->points.size(); ++j)
            {
                // if not in nearby scans, end the loop
                if (int(last_surfPointsLessFlat->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                    break;

                double pointSqDis = (last_surfPointsLessFlat->points[j].x - current_surfPointsSharp->points[i].x) *
                                        (last_surfPointsLessFlat->points[j].x - current_surfPointsSharp->points[i].x) +
                                    (last_surfPointsLessFlat->points[j].y - current_surfPointsSharp->points[i].y) *
                                        (last_surfPointsLessFlat->points[j].y - current_surfPointsSharp->points[i].y) +
                                    (last_surfPointsLessFlat->points[j].z - current_surfPointsSharp->points[i].z) *
                                        (last_surfPointsLessFlat->points[j].z - current_surfPointsSharp->points[i].z);

                // if in the same or lower scan line
                if (int(last_surfPointsLessFlat->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
                {
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
                // if in the higher scan line
                else if (int(last_surfPointsLessFlat->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
                {
                    minPointSqDis3 = pointSqDis;
                    minPointInd3 = j;
                }
            }
            for (int j = closestPointInd - 1; j >= 0; --j)
            {
                // if not in nearby scans, end the loop
                if (int(last_surfPointsLessFlat->points[j].intensity) < (closestPointScanID + NEARBY_SCAN))
                    break;

                double pointSqDis = (last_surfPointsLessFlat->points[j].x - current_surfPointsSharp->points[i].x) *
                                        (last_surfPointsLessFlat->points[j].x - current_surfPointsSharp->points[i].x) +
                                    (last_surfPointsLessFlat->points[j].y - current_surfPointsSharp->points[i].y) *
                                        (last_surfPointsLessFlat->points[j].y - current_surfPointsSharp->points[i].y) +
                                    (last_surfPointsLessFlat->points[j].z - current_surfPointsSharp->points[i].z) *
                                        (last_surfPointsLessFlat->points[j].z - current_surfPointsSharp->points[i].z);

                // if in the same or lower scan line
                if (int(last_surfPointsLessFlat->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
                {
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
                // if in the higher scan line
                else if (int(last_surfPointsLessFlat->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
                {
                    minPointSqDis3 = pointSqDis;
                    minPointInd3 = j;
                }
            }
            if (minPointInd2 >= 0 && minPointInd3 >= 0)
            {

                Eigen::Vector3d curr_point(current_surfPointsSharp->points[i].x,
                                            current_surfPointsSharp->points[i].y,
                                            current_surfPointsSharp->points[i].z);
                curr_surfpoints.push_back(curr_point);
                Eigen::Vector3d last_point_a(last_surfPointsLessFlat->points[closestPointInd].x,
                                                last_surfPointsLessFlat->points[closestPointInd].y,
                                                last_surfPointsLessFlat->points[closestPointInd].z);
                last_surfpoints_a.push_back(last_point_a);
                Eigen::Vector3d last_point_b(last_surfPointsLessFlat->points[minPointInd2].x,
                                                last_surfPointsLessFlat->points[minPointInd2].y,
                                                last_surfPointsLessFlat->points[minPointInd2].z);
                last_surfpoints_b.push_back(last_point_b);
                Eigen::Vector3d last_point_c(last_surfPointsLessFlat->points[minPointInd3].x,
                                                last_surfPointsLessFlat->points[minPointInd3].y,
                                                last_surfPointsLessFlat->points[minPointInd3].z);
                last_surfpoints_c.push_back(last_point_c);
                surf_correspondence++;
            }
        
        }      
     }
     return true;
}
bool LOAMRegistration::IsSignificant(
    const Eigen::Matrix4f &transformation,
    const float trans_eps
) {
    // a. translation magnitude -- norm:
    float translation_magnitude = transformation.block<3, 1>(0, 3).norm();
    // b. rotation magnitude -- angle:
    float rotation_magnitude = fabs(
        acos(
            (transformation.block<3, 3>(0, 0).trace() - 1.0f) / 2.0f
        )
    );

    return (
        (translation_magnitude > trans_eps) || 
        (rotation_magnitude > trans_eps)
    );
}